#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <ctype.h>
#include <unistd.h>
#include "common.h"
#include "EnrolleeSM.h"
#include "wsc_supplicant.h"
#include "aes.h"
#include "randomlib.h"
#include "crypto_internal.h"
#include "ieee80211_ioctl.h"
#include <netinet/in.h>
#include <time.h>

int connect_start_time;
struct simpleconfigS simpleconfigS;

#define debugf    printf

extern void sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len,
                 u8 *mac);
extern int set80211param(struct wsc_drv *wsc_drv, int op, int arg, int show_err);
extern void l2_packet_deinit(struct l2_packet_data *l2);
extern int wps_driver_zydas_wps_filter(struct wsc_drv *drv, int enable);
extern int zydas_set_param(struct wsc_drv *drv, int op, int arg, int show_err);

void simpleconfigNewState(enum simpleconfigState NewState)
{
    if ( NewState == 0 && NewState == simpleconfigS.State )
    {
        return;
    }

    debugf("simpleconfigNewState STATE CHANGE %d==%s to %d==%s\n",
        simpleconfigS.State, 
        simpleconfigStateNames[simpleconfigS.State],
        NewState,
        simpleconfigStateNames[NewState]
    );

    simpleconfigS.State = NewState;
    simpleconfigS.NTries = 0;

    /* TODO: Start a timer */
    //pollerTaskReady(&simpleconfigS.Timer);

    //if ( simpleconfigS.StatusCB )
    //{
    //    (*simpleconfigS.StatusCB)();
    //}
}

/* The HMAC algorithm is defined in RFC2104.
 *      It is a meta-algorithm, that can be used with a variety of
 *      hash algorithms.
 *      Simpleconfig uses the SHA256 hash algorithm.
 *      The SHA256 hash algorithm takes arbitrarly long byte arrays
 *      and produces a 32 byte hash.
 *      For convenience, the byte array may be "vectored" i.e.
 *      can be presented to the sha256_vector() function as a set
 *      of pieces.
 *
 *      Almost all of this function is HMAC code as defined in RFC2104; 
 *      the only SHA256 dependencies are the use of sha256_vector function, 
 *      and the assumption that the output size of the hash function
 *      is 32 bytes.
 */
void simpleconfigHmacSHA256( 
        const void* key, 
        int klen, 
        int num_elems,  /* specifying vec[1]...vec[num_elems] */
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
         * so that vec[0] and vlen[0] can be scribbled over.
         */
        //const unsigned char *vec[num_elems+1],
        //size_t vlen[num_elems+1],
        const unsigned char **vec,
        size_t *vlen,
        unsigned char md[SIMPLECONFIG_SHA256_OUTPUT_SIZE]
        )
{
    unsigned char k_ipad[(SIMPLECONFIG_HMAC_BLOCK_SIZE+1)];
    unsigned char k_opad[(SIMPLECONFIG_HMAC_BLOCK_SIZE+1)];
    unsigned char tkey[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    unsigned char inner_md[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int         i;
    // For vectoring:
    const unsigned char *addr[4];
    size_t alen[4];

    // if key is longer than 64 bytes reset it to key = SHA256(key)
    if (klen > SIMPLECONFIG_HMAC_BLOCK_SIZE)
    {
        addr[0] = key;
        alen[0] = klen;
        sha256_vector( 
            1,  // num_elem
            addr,
            alen,
            tkey
            );
        key = tkey;
        klen = SIMPLECONFIG_SHA256_OUTPUT_SIZE;
    }

    memset(k_ipad, 0, sizeof(k_ipad));
    memset(k_opad, 0, sizeof(k_opad));
    memcpy(k_ipad, key, klen);
    memcpy(k_opad, key, klen);

    // XOR key with ipad and opad values
    for (i = 0; i < SIMPLECONFIG_HMAC_BLOCK_SIZE; i++)
    {
        k_ipad[i] ^= 0x36;
        k_opad[i] ^= 0x5C;
    }

    // perform inner SHA256
    // Note that (as documented above) we add in vector element 0.
    vec[0] = k_ipad;
    vlen[0] = SIMPLECONFIG_HMAC_BLOCK_SIZE;
    sha256_vector( 
        num_elems+1,
        vec,
        vlen,
        inner_md
        );

    // perform outer SHA256
    addr[0] = k_opad;
    alen[0] = SIMPLECONFIG_HMAC_BLOCK_SIZE;
    addr[1] = inner_md;
    alen[1] = SIMPLECONFIG_SHA256_OUTPUT_SIZE;
    sha256_vector( 
        2,  // num_elem
        addr,
        alen,
        md
        );
}

/* simpleconfigWrap does keywrapping of  message components.
 * The output (crypt) begins with 16 byte initialization vector ("IV")
 * which is necessary for decrypting the remaining bytes using
 * the key wrap key.
 * Return value is number of output bytes.
 */
int simpleconfigWrap(
        unsigned char *plain,           /* in: decrypted bytes */
        int plainlen,                   /* in: size */
        unsigned char *crypt            /* output IV and encrypted bytes */
        )
{
    unsigned char *startCrypt = crypt;
    unsigned char *LastCrypt = crypt;

    void *aesHandle = aes_encrypt_init(
        simpleconfigS.KeyWrapKey, sizeof(simpleconfigS.KeyWrapKey));
    assert( aesHandle != NULL );

    /* Get random initialization vector, stick at front of output.
     * This is used as the seed for the CBC algorithm (see below).
     */
    randomlibGetArray( crypt, SIMPLECONFIG_AES_BLOCK_SIZE );
    crypt += SIMPLECONFIG_AES_BLOCK_SIZE;

    for (;; ) {
        int thislen = plainlen;
        int i;
        if ( thislen > SIMPLECONFIG_AES_BLOCK_SIZE )
            thislen = SIMPLECONFIG_AES_BLOCK_SIZE;
        unsigned char BlockPlain[SIMPLECONFIG_AES_BLOCK_SIZE];
        memcpy( BlockPlain, plain, thislen );
        if ( thislen < SIMPLECONFIG_AES_BLOCK_SIZE ) {
            /* Last block: pad out with a byte value that gives the number
             * of padding bytes.
             */
            int ipad;
            int npad = SIMPLECONFIG_AES_BLOCK_SIZE - thislen;
            for ( ipad = 0; ipad < npad; ipad++ ) {
                BlockPlain[SIMPLECONFIG_AES_BLOCK_SIZE-ipad-1] = npad;
            }
        }
        /* Cipher Block Chaining (CBC) -- xor the plain text with
         * the last AES output (or initially, the "initialization vector").
         */

        for ( i = 0; i < SIMPLECONFIG_AES_BLOCK_SIZE; i++ ) {
            BlockPlain[i] ^= LastCrypt[i];
        }
        /* And encrypt and store in output */
        aes_encrypt( aesHandle, BlockPlain, crypt );
        LastCrypt = crypt;
        crypt += SIMPLECONFIG_AES_BLOCK_SIZE;
        if ( thislen < SIMPLECONFIG_AES_BLOCK_SIZE ) break;
        plain += SIMPLECONFIG_AES_BLOCK_SIZE;
        plainlen -= SIMPLECONFIG_AES_BLOCK_SIZE;
    }
    aes_decrypt_deinit( aesHandle );    /* free resources */
    return crypt - startCrypt;
}

/* simpleconfigUnWrap undoes keywrapping of received message components.
 * The input (crypt) begins with 16 byte initialization vector ("IV")
 * which is necessary for decrypting the remaining bytes using
 * the key wrap key.
 *
 * Returns no. of output bytes.
 */
int simpleconfigUnWrap(
        const unsigned char *crypt,     /* IV and encrypted bytes */
        int cryptlen,                   /* n. encrypted bytes incl. IV */
        unsigned char *plain            /* out: decrypted bytes */
        )
{
    unsigned char *plainStart = plain;

    void *aesHandle = aes_decrypt_init(
        simpleconfigS.KeyWrapKey, sizeof(simpleconfigS.KeyWrapKey));
    assert( aesHandle != NULL );

    /* Initialization vector is always the first block */
    unsigned const char *LastBlockCrypt = crypt;
    crypt += SIMPLECONFIG_AES_BLOCK_SIZE;
    cryptlen -= SIMPLECONFIG_AES_BLOCK_SIZE;

    while ( cryptlen >= SIMPLECONFIG_AES_BLOCK_SIZE ) {
        int i;
        unsigned char BlockPlain[SIMPLECONFIG_AES_BLOCK_SIZE];
        aes_decrypt( aesHandle, crypt, BlockPlain );
        /* Cipher Block Chaining (CBC) -- xor the plain text with
         * the last AES output (or initially, the "initialization vector").
         */
        for ( i = 0; i < SIMPLECONFIG_AES_BLOCK_SIZE; i++ ) {
            BlockPlain[i] ^= LastBlockCrypt[i];
        }
        LastBlockCrypt = crypt;
        crypt += SIMPLECONFIG_AES_BLOCK_SIZE;
        cryptlen -= SIMPLECONFIG_AES_BLOCK_SIZE;
        int thislen = SIMPLECONFIG_AES_BLOCK_SIZE;
        if ( cryptlen < SIMPLECONFIG_AES_BLOCK_SIZE ) {
            /* Cryptlen SHOULD be zero at this point.
             * If not, try to carry on.
             */
            if ( cryptlen ) {
                printf(
                "simpleconfigUnWrap: OUCH: %d bytes left over.\n", cryptlen );
            }
            /* Padding SHOULD give no. of pad chars, and SHOULD
             * be between 1 and SIMPLECONFIG_AES_BLOCK_SIZE;
             * if not, try to carry on.
             */
            int npad = BlockPlain[SIMPLECONFIG_AES_BLOCK_SIZE-1];
            if ( npad <= 0 || npad > SIMPLECONFIG_AES_BLOCK_SIZE ) {
                printf("simpleconfigUnWrap: OUCH: npad==%d\n", npad);
                npad = 0;
            } else {
                thislen -= npad;
            }
        }
        memcpy( plain, BlockPlain, thislen );
        plain += thislen;
    }
    aes_decrypt_deinit( aesHandle );

    return plain - plainStart;
}

void simpleconfigMsgDump(
    const char *Label,
    const void *inbuf, 
    int inlen
    )
{
    debugf("%s simpleconfigMsgDump begin (len=%d)\n", Label, inlen);

    const unsigned char *buf = inbuf;
    int buflen = inlen;
    /* For now, don't dump the header */
    buf += sizeof( struct simpleconfigEapMessageHeader );
    buflen -= sizeof( struct simpleconfigEapMessageHeader );

    while ( buflen > 4 ) {
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            debugf("%s ERROR elem 0x%x len %d > buflen==%d\n",
                Label, elemid, elemlen, buflen );
            return;
        }
        switch ( elemlen ) {
            case 1:
                debugf("%s elem 0x%x : 0x%02x\n",
                    Label, elemid, buf[0] );
            break;
            case 2:
                debugf("%s elem 0x%x : 0x%02x%02x\n",
                    Label, elemid, buf[0], buf[1] );
            break;
            case 3:
                debugf("%s elem 0x%x : 0x%02x%02x%02x\n",
                    Label, elemid, buf[0], buf[1], buf[2] );
            break;
            case 4:
                debugf("%s elem 0x%x : 0x%02x%02x%02x%02x\n",
                    Label, elemid, buf[0], buf[1], buf[2], buf[3] );
            break;
            case 5:
                debugf("%s elem 0x%x : 0x%02x%02x%02x%02x%02x\n",
                    Label, elemid, buf[0], buf[1], buf[2], buf[3], buf[4] );
            break;
            case 6:
                debugf("%s elem 0x%x : 0x%02x%02x%02x%02x%02x%02x\n",
                    Label, elemid, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5] );
            break;
            case 7:
                debugf("%s elem 0x%x : 0x%02x%02x%02x%02x%02x%02x%02x\n",
                    Label, elemid, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6] );
            break;
            case 16:    /* a common case, e.g. for nonces */
                debugf("%s elem 0x%x : 0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
                    Label, elemid, 
                    buf[0], buf[1], buf[2], buf[3], 
                    buf[4], buf[5], buf[6], buf[7],
                    buf[8], buf[9], buf[10], buf[11], 
                    buf[12], buf[13], buf[14], buf[15] 
                    );
            break;
            default: /* 8 or more */
                debugf("%s elem 0x%x (%dB): 0x%02x%02x%02x%02x%02x%02x%02x%02x...\n",
                    Label, elemid, elemlen,
                    buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7] );
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
    }
    debugf("%s simpleconfigMsgDump end\n", Label);
    return;
}

void simpleconfigMsgDump1(
    const char *Label,
    int Offset,         /* printed out */
    const void *inbuf, 
    int inlen
)
{
    const unsigned char *buf = inbuf;
    int buflen = inlen;

    if ( buflen > 4 )
    {
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;

        if ( elemlen > buflen ) {
            debugf("%s ERROR off %d elem 0x%x len %d > buflen==%d\n",
                Label, Offset, elemid, elemlen, buflen );
            return;
        }
        switch ( elemlen ) {
            case 1:
                debugf("%s off %d elem 0x%x : 0x%02x\n",
                    Label, Offset, elemid, buf[0] );
            break;
            case 2:
                debugf("%s off %d elem 0x%x : 0x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1] );
            break;
            case 3:
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1], buf[2] );
            break;
            case 4:
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1], buf[2], buf[3] );
            break;
            case 5:
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1], buf[2], buf[3], buf[4] );
            break;
            case 6:
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x%02x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5] );
            break;
            case 7:
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x%02x%02x%02x%02x\n",
                    Label, Offset, elemid, buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6] );
            break;
            case 16:    /* a common case, e.g. for nonces */
                debugf("%s off %d elem 0x%x : 0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
                    Label, Offset, elemid, 
                    buf[0], buf[1], buf[2], buf[3], 
                    buf[4], buf[5], buf[6], buf[7],
                    buf[8], buf[9], buf[10], buf[11], 
                    buf[12], buf[13], buf[14], buf[15] 
                    );
            break;
            default:    /* 8 or more.... */
                debugf("%s off %d elem 0x%x (%dB): 0x%02x%02x%02x%02x%02x%02x%02x%02x...\n",
                    Label, Offset, elemid, elemlen,
                    buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7] );
            break;
        }
    }
    return;
}

int simpleconfigDumpProfile(int i)
{
    int iLen, securityMode = wlanProfileSecurityMode_eOpen;

    if (simpleconfigS.State != simpleconfigState_eDone) {
        debugf("simpleconfigMaybeDone: FAILURE (not in eDone mode)!\n");
        goto Restart;
    }
    //struct wlanProfile *profile0 = wlanProfileGetCurrent();
    /* NOTE: reasonable defaults have been set already */

    /* First the simple stuff */
    if (simpleconfigS.Results[i].Ssid) {
        debugf("simpleconfig results: SSID=%.*s\n", 
            sizeof(simpleconfigS.Results[i].Ssid), simpleconfigS.Results[i].Ssid);
    }
    if ( simpleconfigS.Results[i].WepTransmitKeyIdx >= 0 && 
         simpleconfigS.Results[i].WepTransmitKeyIdx< 4  ) {
        //profile0->keyIdx = simpleconfigS.Results.WepTransmitKeyIdx;
        debugf("simpleconfig results: keyidx=%d\n",
            simpleconfigS.Results[i].WepTransmitKeyIdx+1);
    }
    //profile0->channel = simpleconfigS.Results.ApChannel;

    /* Authentication type and encryption type are bundled
    * for us but separated for SimpleConfig (and for IEEE 802.11 generally).
    * Do the best we can... 
    */
    debugf("simple config results: encrypt=0x%x auth=0x%x\n",
        simpleconfigS.Results[i].EncryptionType,
        simpleconfigS.Results[i].AuthenticationType );
    switch ( simpleconfigS.Results[i].AuthenticationType ) {
        case simpleconfigAuthenticationType_Open    :
            //profile0->securityMode = wlanProfileSecurityMode_eOpen;
            securityMode = wlanProfileSecurityMode_eOpen;
            debugf("simpleconfig results: auth=open -> open mode\n");
        //goto GotSecurityType;
        break;
        case simpleconfigAuthenticationType_WPAPSK  :
            //profile0->securityMode = wlanProfileSecurityMode_eWpaPsk;
            securityMode = wlanProfileSecurityMode_eWpaPsk;
            debugf("simpleconfig results: auth=wpapsk -> wpapsk mode\n");
        //goto GotSecurityType;
        break;
        case simpleconfigAuthenticationType_Shared  :
            //profile0->securityMode = wlanProfileSecurityMode_eWep;
            securityMode = wlanProfileSecurityMode_eWep;
            debugf("simpleconfig results: auth=shared -> wep mode\n");
        //goto GotSecurityType;
        break;
        case simpleconfigAuthenticationType_WPA     :
            debugf("simpleconfig results: auth=wpa (not psk); not supported\n");
        break;
        case simpleconfigAuthenticationType_WPA2    :
            debugf("simpleconfig results: auth=wpa2 (not psk); not supported\n");
        break;
        case simpleconfigAuthenticationType_WPA2PSK :
            //profile0->securityMode = wlanProfileSecurityMode_eWpa2Psk;
            securityMode = wlanProfileSecurityMode_eWpa2Psk;
            debugf("simpleconfig results: auth=wpa2psk -> wpa2psk mode\n");
        //goto GotSecurityType;
        break;
    }
    switch ( simpleconfigS.Results[i].EncryptionType ) {
        case simpleconfigEncryptType_None :
            //profile0->securityMode = wlanProfileSecurityMode_eOpen;
            securityMode = wlanProfileSecurityMode_eOpen;
            debugf("simpleconfig results: no encryption -> open mode\n");
        goto GotSecurityType;
        case simpleconfigEncryptType_WEP  :
            //profile0->securityMode = wlanProfileSecurityMode_eWep;
            securityMode = wlanProfileSecurityMode_eWep;
            debugf("simpleconfig results: encryption=WEP -> WEP mode\n");
        goto GotSecurityType;
        case simpleconfigEncryptType_TKIP :
            //profile0->securityMode = wlanProfileSecurityMode_eWpaPsk;
            securityMode = wlanProfileSecurityMode_eWpaPsk;
            debugf("simpleconfig results: encryption=TKIP -> WPA mode\n");
        goto GotSecurityType;
        case simpleconfigEncryptType_AES  :
        case 0xC : // AES + TKIP
            //profile0->securityMode = wlanProfileSecurityMode_eWpa2Psk;
            securityMode = wlanProfileSecurityMode_eWpa2Psk;
            debugf("simpleconfig results: encryption=AES -> WPA2 mode\n");
        goto GotSecurityType;
    }
    #if 0       // maybe
    if ( simpleconfigS.Results.AuthenticationTypeFlags & 
            simpleconfigAuthenticationType_WPA2PSK ) {
        profile0->securityMode = wlanProfileSecurityMode_eWpa2Psk;
        goto GotSecurityType;
    }
    if ( simpleconfigS.Results.AuthenticationTypeFlags & 
            simpleconfigAuthenticationType_WPAPSK ) {
        profile0->securityMode = wlanProfileSecurityMode_eWpaPsk;
        goto GotSecurityType;
    }
    if ( simpleconfigS.Results.AuthenticationTypeFlags & 
            simpleconfigAuthenticationType_Shared ) {
        profile0->securityMode = wlanProfileSecurityMode_eWep;
        goto GotSecurityType;
    }
    if ( simpleconfigS.Results.AuthenticationTypeFlags & 
            simpleconfigAuthenticationType_Open ) {
        profile0->securityMode = wlanProfileSecurityMode_eOpen;
        goto GotSecurityType;
    }
    #endif
    /* No authentication type that we can get along with. Punt.
     */
    debugf("simpleconfig results: No authentication mode. Ouch!\n");
    goto Restart;

    GotSecurityType:;
    /* The meaning of NetworkKey depends upon the authentication
     *  method, which we represent by our security mode.
     */
    const char *NetworkKey = simpleconfigS.Results[i].NetworkKey;
    int KeyLen = simpleconfigS.Results[i].NetworkKeyLen;

#if 1
    switch ( securityMode )
    {
        case wlanProfileSecurityMode_eOpen:
        goto GotKey;    /* don't care about key */
        case wlanProfileSecurityMode_eWep:
            debugf("simpleconfig results: wep keylen %d\n", KeyLen );
            if ( KeyLen == 5 || KeyLen == 13 ) goto GotAsciiKey;
            if ( KeyLen == 10 || KeyLen == 26 ) goto GotHexKey;
            debugf("simpleconfig results: bad wep keylen %d\n", KeyLen );
            goto Restart;
        break;
        case wlanProfileSecurityMode_eWpaPsk:
        case wlanProfileSecurityMode_eWpa2Psk:
            if ( KeyLen < 64 ) goto GotAsciiKey;
            if ( KeyLen == 64 ) goto GotHexKey;
            debugf("simpleconfig results: bad wpa(2) keylen %d\n", KeyLen );
            goto Restart;
        break;
        default:
            debugf("simpleconfig results: bad security mode\n");
        goto Restart;
    }
    goto Restart;
#endif

    GotAsciiKey:
    /* Must be an ascii key, which really means a passphrase --
     * or for WEP it is the actual binary key --
     * use it as is.
     */
    if ( KeyLen > WLAN_KEY_PASSPHRASE_MAX ) {
        debugf("simple config results: over long ascii key len %d\n", KeyLen);
        goto Restart;
    }
    //profile0->keyMethodHex = 0;
    //profile0->keynbytes = 0;
    //memset( profile0->passphrase, 0, sizeof(profile0->passphrase));
    //memcpy( profile0->passphrase, NetworkKey, KeyLen );
    debugf("simple config results: ascii key is `%.*s'",
        KeyLen, NetworkKey );
    goto GotKey;

    GotHexKey:
    /* Must be hex key. This means NOT a passphrase, but the
     * final network key... must convert from hex to binary however.
     */
    if ( (KeyLen&1) != 0 ) {
        debugf("simpleconfig results: bad odd hex key len %d\n", KeyLen);
        goto Restart;
    }
    if ( KeyLen/2 > WLAN_KEY_PASSPHRASE_MAX ) {
        debugf("simpleconfig results: overlong hex key len %d\n", KeyLen);
        goto Restart;
    }
    //profile0->keyMethodHex = 1;
    //profile0->keynbytes = KeyLen/2;
    for ( iLen = 0; iLen < KeyLen; iLen++ ) {
        int Value;
        int Ch = NetworkKey[iLen];
        if ( ! isxdigit(Ch) ) {
            debugf("simpleconfig results: bad char in hex key\n");
            goto Restart;
        }
        if ( isdigit(Ch) ) Value = Ch - '0';
        else if ( islower(Ch) ) Value = 10 + Ch - 'a';
        else                    Value = 10 + Ch - 'A';
        //if ( (iLen&1) == 0 )
        //    profile0->key[iLen/2] = Value<<4;
        //else 
        //    profile0->key[iLen/2] |= Value;
            
    }
    debugf("simple config results: hex key is `%.*s'\n",
        KeyLen, NetworkKey );
    goto GotKey;

    GotKey:
    goto GotItAll;

    GotItAll:
    memcpy( &simpleconfigS.LastResults, &simpleconfigS.Results[i],
        sizeof(simpleconfigS.LastResults));
    /* no errors! */
    simpleconfigS.LastError = 0;
    simpleconfigS.LastErrMsg[0] = 0;
    // did already above: simpleconfigCancelCurrent();
    strcpy(simpleconfigS.Pin, "00000000");
    simpleconfigS.PushButtonMethod = 0;
    simpleconfigS.Success = 1;
    //simpleconfigCancelCurrent();
    /* End the mode in wlanProfile... accept profile as changed above */
    debugf("simpleconfig results: SUCCESS\n");
    //wlanProfileSimpleConfigDone();
    return 0;

    Restart:
    debugf("simpleconfig results: BAD, starting over.\n");
    return 1;
}

void simpleconfigPacketSend(
        struct wsc_drv *wsc_drv,
        const char *DebugMsg,
        void *buf,
        int nbytes
        )
{

    debugf("simpleconfigPacketSend SENDING %s, nbytes: %d\n", DebugMsg, nbytes);

    wsc_send_udp_data(wsc_drv, buf, nbytes);
}

void simpleconfigDumpNack(const char *Msg, int MsgLen)
{
    const char *buf = Msg;
    int buflen = MsgLen;
    int Offset = 0;

    while (buflen > 4)
    {
        simpleconfigMsgDump1("simpleconfig Rcvd NACK", Offset, buf, buflen);
        // const unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;

        if (elemlen > buflen)
        {
            goto BadElem;
        }

        switch( elemid )
        {
            case simpleconfigMessageElement_ConfigurationError:
            {
                int ErrCode = (buf[0] << 8) | buf[1];

                if ( ErrCode <= 
                        sizeof(simpleconfigConfigurationErrorMessages) /
                        sizeof(simpleconfigConfigurationErrorMessages[0]) &&
                        simpleconfigConfigurationErrorMessages[ErrCode] != NULL )
                {
                    debugf("NACK contains error code %d == %s", 
                            ErrCode,
                            simpleconfigConfigurationErrorMessages[ErrCode]
                          );

                        /* TODO: lang-ize this */
                        //spr(simpleconfigS.LastErrMsg, 
                        //    sizeof(simpleconfigS.LastErrMsg),
                        //    "NACK ERR %s", 
                        //    simpleconfigConfigurationErrorMessages[ErrCode] );
                }
                else
                {
                    debugf("NACK contains error code %d", ErrCode );
                    /* TODO: lang-ize this */
                    //spr(simpleconfigS.LastErrMsg, 
                    //    sizeof(simpleconfigS.LastErrMsg),
                    //    "NACK ERR %d", 
                    //    ErrCode );
                }
            }
            break;
        }

        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }

    BadElem:;
}

void simpleconfigSessionKeyGen1Pre(void)
{
    /* Build:
     * -- a new nonce for ourself in simpleconfigS.OurNonce.
     * -- a DH key pair ; DH public key
     */
    size_t Len;

    randomlibGetArray(simpleconfigS.OurNonce, 
        sizeof(simpleconfigS.OurNonce));
    randomlibGetArray(simpleconfigS.OurPrivateKey, 
        sizeof(simpleconfigS.OurPrivateKey));
    /* crypto_mod_exp is used from (wpa_)supplicant library */
    simpleconfigS.OurPublicKeyLen = sizeof(simpleconfigS.OurPublicKey); /*max*/
    // Generate public key from our private key.....
    //  == g^A mod p where A is our private key.
    Len = sizeof(simpleconfigS.OurPublicKey);    /* input, max */
    int CryptoError = crypto_mod_exp(
        simpleconfigDhG,
        sizeof(simpleconfigDhG),
        simpleconfigS.OurPrivateKey,
        sizeof(simpleconfigS.OurPrivateKey),
        simpleconfigDhP,
        sizeof(simpleconfigDhP),
        simpleconfigS.OurPublicKey,     /* output */
        &Len                            /* in/out */
        );
    simpleconfigS.OurPublicKeyLen = Len;
    assert(CryptoError==0);     /* abort if we mis-sized */
}

/*
 * Required once only per session, at end of message M2 parsing.
 * Requires receipt of registrar's public key.
 */
void simpleconfigSessionKeyGen2Post(void)
{
    const unsigned char *vec[6];
    size_t vlen[6];
    int iter;

    /* Calculate the Diffie-Hellman shared secret g^AB mod p
     * by calculating (PKr)^A mod p
     */
    /* crypto_mod_exp is used from (wpa_)supplicant library */
    size_t Len = sizeof(simpleconfigS.DHSecret);        /* input, max */
    int CryptoError = crypto_mod_exp(
        simpleconfigS.RegistrarPublicKey,
        simpleconfigS.RegistrarPublicKeyLen,
        simpleconfigS.OurPrivateKey,
        sizeof(simpleconfigS.OurPrivateKey),
        simpleconfigDhP,
        sizeof(simpleconfigDhP),
        simpleconfigS.DHSecret,         /* output */
        &Len                            /* in/out */
        );
    simpleconfigS.DHSecretLen = Len;
    assert(CryptoError==0);     /* abort if we didn't have enough room */

    /* Calculate DHKey (hash of DHSecret)
     */
    vec[0] = simpleconfigS.DHSecret;
    vlen[0] = simpleconfigS.DHSecretLen;
    sha256_vector( 
        1,  // num_elem
        vec,
        vlen,
        simpleconfigS.DHKey
        );

    /* Calculate KDK (Key Derivation Key)
     */
    vec[1] = simpleconfigS.OurNonce;
    vlen[1] = sizeof(simpleconfigS.OurNonce);
    vec[2] = simpleconfigS.OurAddr;
    vlen[2] = sizeof(simpleconfigS.OurAddr);
    vec[3] = simpleconfigS.RegistrarNonce;
    vlen[3] = sizeof(simpleconfigS.RegistrarNonce);
    simpleconfigHmacSHA256( 
        simpleconfigS.DHKey,
        sizeof(simpleconfigS.DHKey),
        3,      /* num_elems */
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
         * so that vec[0] and vlen[0] can be scribbled over.
         */
        vec,
        vlen,
        simpleconfigS.KDK
        );
    
    /* Calculate AuthKey, KeyWrapKey and EMSK
     * using the "key derivation function" ("kdf").
     * Number of iterations is described in the WPA document as:
     * iterations = (total_key_bits + prf_digest_size ?1)/prf_digest_size
     */
    #define simpleconfigKDFNIterations 3
    unsigned char kdfbuf[simpleconfigKDFNIterations][SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int cb2 = htonl(640/*no. of output bits*/);
    vec[2] = "Wi-Fi Easy and Secure Key Derivation";
    vlen[2] = strlen( vec[2] );
    vec[3] = (char *)&cb2;
    vlen[3] = sizeof(cb2);
    for ( iter = 0; iter < simpleconfigKDFNIterations; iter++ ) {
        int cb1 = htonl( iter+1 );
        vec[1] = (char *)&cb1;
        vlen [1] = sizeof(cb1);
        simpleconfigHmacSHA256( 
            simpleconfigS.KDK,
            sizeof(simpleconfigS.KDK),
            3,      /* num_elems */
            /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
            * so that vec[0] and vlen[0] can be scribbled over.
            */
            vec,
            vlen,
            kdfbuf[iter]
            );
    }
    /* 32 bytes for AuthKey */
    unsigned char *kdfout = (void *)kdfbuf;
    memcpy(simpleconfigS.AuthKey, kdfout, sizeof(simpleconfigS.AuthKey));
    kdfout += sizeof(simpleconfigS.AuthKey);
    /* 16 bytes for KeyWrapKey */
    memcpy(simpleconfigS.KeyWrapKey, kdfout, sizeof(simpleconfigS.KeyWrapKey));
    kdfout += sizeof(simpleconfigS.KeyWrapKey);
    #if 0       // We don't need EMSK 
    /* 32 bytes for EMSK */
    /* NOTE: jswscsup/common.cpp function DeriveKeys mistakenly(?)
     * copies out the EMSK at wrong location.
     * But EMSK is only used on registrar side...
     */
    memcpy(simpleconfigS.EMSK, kdfout, sizeof(simpleconfigS.EMSK));
    #endif

    /* Calculate Psk1, Psk2, EHash1, EHash2
     */
    randomlibGetArray(simpleconfigS.ENonce1, sizeof(simpleconfigS.ENonce1));
    randomlibGetArray(simpleconfigS.ENonce2, sizeof(simpleconfigS.ENonce2));
    // NOTE: In pushbutton case, PIN has been set to 00000000
    char *Password = simpleconfigS.Pin;
    int PasswordLen = strlen(Password);
    assert(PasswordLen > 0);
    int Psk1PasswordLen = (PasswordLen+1)/2;
    
    // PSK1
    vec[1] = Password;
    vlen[1] = Psk1PasswordLen;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        1,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        simpleconfigS.Psk1
        );

    // PSK2
    vec[1] = Password + Psk1PasswordLen;
    vlen[1] = PasswordLen - Psk1PasswordLen;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        1,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        simpleconfigS.Psk2
        );

    // EHash1
    vec[1] = simpleconfigS.ENonce1;
    vlen[1] = SIMPLECONFIG_NONCE_SIZE;
    vec[2] = simpleconfigS.Psk1;
    vlen[2] = SIMPLECONFIG_NONCE_SIZE;        /* first 16 bytes only */
    vec[3] =  simpleconfigS.OurPublicKey;
    vlen[3] = SIMPLECONFIG_PUBLIC_KEY_SIZE,
    vec[4] =  simpleconfigS.RegistrarPublicKey;
    vlen[4] =  SIMPLECONFIG_PUBLIC_KEY_SIZE,
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        4,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        simpleconfigS.EHash1
        );

    // EHash2
    vec[1] = simpleconfigS.ENonce2;
    vlen[1] = SIMPLECONFIG_NONCE_SIZE;
    vec[2] = simpleconfigS.Psk2;        /* first 16 bytes only */
    vlen[2] = SIMPLECONFIG_NONCE_SIZE;
    vec[3] =  simpleconfigS.OurPublicKey;
    vlen[3] = SIMPLECONFIG_PUBLIC_KEY_SIZE,
    vec[4] =  simpleconfigS.RegistrarPublicKey;
    vlen[4] =  SIMPLECONFIG_PUBLIC_KEY_SIZE,
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        4,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        simpleconfigS.EHash2
        );
}

/* simpleconfigBuildEapHeader -- fill in the headers for eap messages.
 *      This includes ethernet, eapol and eap headers.
 *      The remainder of the packet is built first (so we can know how
 *      long it is) and the this is called to fill in the header
 *      (requires knowing the length).
 */
void simpleconfigBuildEapHeader(
        void *outbuf,       /* must be big enough */
        int TotalLength,
        int EapIdentifier,
        int EapCode
        )
{
    struct simpleconfigEapMessageHeader *hdr = (void *)outbuf;
#if 0
    /* Big endian...... */

    /* Shared code: */
    simpleconfigBuildEapolHeader( outbuf, simpleconfigEapolPacketType_EAP);

    /* EAPOL Header (4 bytes): */
    hdr->EapolPacketBodyLength = ntohs(TotalLength - (14+4)); /*excludes eapol hdr*/
#endif

    /* basic EAP Header: */
    hdr->EapCode = EapCode;
    hdr->EapIdentifier = EapIdentifier;
    //hdr->EapLength = ntohs(TotalLength - simpleconfigEapStartOffset);  /* includes eap hdr */
    hdr->EapLength = ntohs(TotalLength);
    return;
}

/* simpleconfigBuildEapResponseHeader -- fill in the headers for eap response.
 *      This includes ethernet, eapol and eap headers.
 *      The remainder of the packet is built first (so we can know how
 *      long it is) and the this is called to fill in the header
 *      (requires knowing the length).
 */
void simpleconfigBuildEapResponseHeader(
        void *outbuf,       /* must be big enough */
        int TotalLength,
        int EapIdentifier,
        int EapType
        )
{
    struct simpleconfigEapMessageHeader *hdr = (void *)outbuf;
    /* Big endian...... */

    /* Shared code: */
    simpleconfigBuildEapHeader( 
        outbuf, 
        TotalLength,
        EapIdentifier,
        simpleconfigEapCode_Response
        );

    /* Expanded type fields: */
    hdr->EapType = EapType;
    return;
}

/* simpleconfigBuildEapExpandedHeader -- fill in the headers for eap response.
 *      This includes ethernet, eapol and eap headers.
 *      The remainder of the packet is built first (so we can know how
 *      long it is) and the this is called to fill in the header
 *      (requires knowing the length).
 */
void simpleconfigBuildEapExpandedHeader(
        void *outbuf,       /* must be big enough */
        int TotalLength,
        int EapIdentifier
        )
{
    struct simpleconfigEapMessageHeader *hdr = (void *)outbuf;
    /* Big endian...... */

    /* Shared code: */
    simpleconfigBuildEapResponseHeader( 
        outbuf, 
        TotalLength,
        EapIdentifier,
        simpleconfigEapType_Expanded
        );

    /* Expanded type fields: */
    hdr->EapVendorId[0] = (simpleconfigHeaderVendorId>>16)&0xFF;
    hdr->EapVendorId[1] = (simpleconfigHeaderVendorId>>8)&0xFF;
    hdr->EapVendorId[2] = (simpleconfigHeaderVendorId)&0xFF;
    hdr->EapVendorTypeHigh = 0;
    hdr->EapVendorTypeLow = htons(simpleconfigEapVendorType_SimpleConfig);
    return;
}


/* simpleconfigBuildSimpleConfigHeader -- fill in the headers for 
 *      SimpleConfig packets.
 *      This includes ethernet, eapol and eap headers.
 *      The remainder of the packet is built first (so we can know how
 *      long it is) and the this is called to fill in the header
 *      (requires knowing the length).
 */
void simpleconfigBuildSimpleConfigHeader(
        void *outbuf,       /* must be big enough */
        int TotalLength,
        int EapIdentifier,
        int SimpleConfigOpCode
        )
{
    struct simpleconfigEapMessageHeader *hdr = (void *)outbuf;
    /* Big endian...... */

    /* Shared code: */
    simpleconfigBuildEapExpandedHeader(
        outbuf,
        TotalLength,
        EapIdentifier
        );

    hdr->SimpleConfigOpCode = SimpleConfigOpCode;
    /* Assume no outgoing fragmentation, no flags needed */
    hdr->SimpleConfigFlags = 0;
    return;
}

void simpleconfigStartSend(void)
{
    /* TODO: Send packet to wpa_supplicant through UDP socket */
}

/*
 * Put a message element header into the message.
 * Returns the pointer into message following this header.
 */
static inline unsigned char * simpleconfigBuildMessageElementHeader(
    unsigned char *buf,
    int AttributeId,
    int DataLength
    )
{
    /* Big endian...... */
    *buf++ = AttributeId >> 8;
    *buf++ = AttributeId;
    *buf++ = DataLength >> 8;
    *buf++ = DataLength;
    return buf;
}

int simpleconfigBuildM1( /* returns no. of bytes put in buffer */
    struct wsc_drv *wsc_drv,
    void *outbuf,       /* must be big enough */
    int serviceType     /* service type of STA */
)
{
    unsigned char *buf = outbuf;
    const char *str;    /* temp variable */
    int len;            /* temp variable */
    unsigned value;     /* temp variable */
    unsigned char *Mac;
    int TotalLength;
    char pUUID[]={0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};

    /* Now is the appropriate time to generate new random keys */
    //simpleconfigSessionKeyGen1Pre();

    /* Come back later and put in the message header */
    buf += sizeof(struct simpleconfigEapMessageHeader);

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_Version, 1);
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_MessageType, 1);
    *buf++ = simpleconfigMessageType_M1;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_UuidE, SIMPLECONFIG_UIID_SIZE);
    //memcpy(buf, simpleconfigS.OurUuid, SIMPLECONFIG_UIID_SIZE);
    if(wsc_drv->CONFIG_DEVICE_UUID == NULL)
        memcpy(buf, pUUID, SIMPLECONFIG_UIID_SIZE);
    else
        memcpy(buf, wsc_drv->CONFIG_DEVICE_UUID, SIMPLECONFIG_UIID_SIZE);
    buf += SIMPLECONFIG_UIID_SIZE;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_MacAddress, 6);
    memcpy(buf, simpleconfigS.OurAddr, 6);
    buf += 6;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_EnrolleeNonce, SIMPLECONFIG_NONCE_SIZE);
    memcpy(buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE);
    buf += SIMPLECONFIG_NONCE_SIZE;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_PublicKey,
        simpleconfigS.OurPublicKeyLen);
    memcpy(buf, simpleconfigS.OurPublicKey, simpleconfigS.OurPublicKeyLen);
    buf += simpleconfigS.OurPublicKeyLen;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_AuthenticationTypeFlags, 2);
    /* Here we give our capabilities: */
    *buf++ = 0; /* high byte */
    *buf++ = 0 
        | 0x1   /* Open */
        | 0x2   /* WPAPSK */
        | 0x4   /* Shared */
        // | 0x8   /* WPA */
        // | 0x10  /* WPA2 */
        | 0x20  /* WPA2PSK */
        ;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_EncryptionTypeFlags, 2);
    /* Here we give our capabilities: */
    *buf++ = 0; /* high byte */
    *buf++ = 0 
        | 0x1   /* None */
        | 0x2   /* WEP */
        | 0x4   /* TKIP */
        | 0x8   /* AES */
        ;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_ConnectionTypeFlags, 1);
    *buf++ = 0
        /* ESS is extended service set, i.e. APs with roaming.
         */
        | 0x1   /* ESS */
        /* 
         * IBSS :== independent basic service set (peer to peer) vs. 
         * infrastructure basic service set (AP with no roaming).
         * We do in theory support peer-to-peer so i'll leave this in.
         */
        | 0x2   /* IBSS */
        ;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_ConfigMethods, 2 );
    value = 0
        /* Just a guess on which should be enabled: */
        // | 0x1   /* USBA (flash drive) */
        // | 0x2   /* ethernet */
        | 0x4   /* Label */
        | 0x8   /* Display */
        // | 0x10  /* external nfc token */
        // | 0x20  /* integrated nfc token */
        // | 0x40  /* nfc interface */
        | 0x80  /* push button */
        | 0x100 /* key pad */
        ;
    *buf++ = (value >> 8)&0xFF;
    *buf++ = (value)&0xFF;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_WiFiProtectedSetupState, 1);
    /* (referred to as "ScState" in reference implementation,
     * i.e. simpleconfig state)
     */
    *buf++ = 1 /* not configured */;

    //str = CFG_SIMPLE_CONFIG_MANUFACTURER;
    str = wsc_drv->CONFIG_MANUFACTURER;
    if(str == NULL) str = CFG_SIMPLE_CONFIG_MANUFACTURER;
    len = strlen(str);
    if (len > 64)
        len = 64;
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_Manufacturer, len);
    memcpy( buf, str, len );
    buf += len;
    
    //str = CFG_SIMPLE_CONFIG_MODEL_NAME;
    str = wsc_drv->CONFIG_MODEL_NAME;
    if(str == NULL) str = CFG_SIMPLE_CONFIG_MODEL_NAME;
    len = strlen(str);
    if ( len > 64 )
        len = 64;
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_ModelName, len);
    memcpy(buf, str, len);
    buf += len;
    
    //str = CFG_SIMPLE_CONFIG_MODEL_NUMBER;
    str = wsc_drv->CONFIG_MODEL_NUMBER;
    if(str == NULL) str = CFG_SIMPLE_CONFIG_MODEL_NUMBER;
    len = strlen(str);
    if (len > 32)
        len = 32;
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_ModelNumber, len);
    memcpy( buf, str, len );
    buf += len;
    
    /* Use mac address as serial number */
    Mac = simpleconfigS.OurAddr;
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_SerialNumber, 
        17 /*mac adrs representation len; could be up to 32*/);

    sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x",
        Mac[0], Mac[1], Mac[2], Mac[3], Mac[4], Mac[5]);
    buf += 17;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_PrimaryDeviceType, 8);
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[0];   
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[1];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[2];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[3];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[4];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[5];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[6];
    *buf++ = wsc_drv->CONFIG_PRIMARY_DEVICE_TYPE[7];
 
    //str = CFG_SIMPLE_CONFIG_DEVICE_NAME;
    str = wsc_drv->CONFIG_DEVICE_NAME;
    if(str == NULL) str = CFG_SIMPLE_CONFIG_DEVICE_NAME;
    len = strlen(str);
    if (len > 32)
       len = 32;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_DeviceName, len);
    memcpy(buf, str, len);
    buf += len;
    
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_RFBands, 1);
    if((wsc_drv->CONFIG_RFBANDS & 3) == 0)
        *buf++ = 1;
    else
        *buf++ = wsc_drv->CONFIG_RFBANDS & 3;
        //| 0x1   /* 2.4 GHz */
        // | 0x2   /* 5.0 GHz */
        //;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_AssociationState, 2);
    /* 0=Not associated, 1 = connection success, 2= configuration failure,
     * 3 = association failure, 4 = IP failure.
     * HMMMM ..... assume we are not associated?
     * The WSC reference always uses 0.
     */
    *buf++ = 0;
    *buf++ = 0;
        
    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_DevicePasswordId, 2);
    /* This is the device password generation method.
     * 0=PIN method (user has entered pin from AP into handset),
     * 1 = User-specified, 2 = machine-specified, 3=rekey,
     * 4 = Push button, 5 = registrar specified.
     *
     * Presumeably we are using only PIN==0 and PushButton==4.
     */
    *buf++ = 0;
    *buf++ = (simpleconfigS.PushButtonMethod ? 
        simpleconfigPasswordId_PushButton : simpleconfigPasswordId_PIN);

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_ConfigurationError, 2);
    /* Presumeably there is no need to report errors here? */
    *buf++ = 0;
    *buf++ = 0;

    buf = simpleconfigBuildMessageElementHeader(buf,
        simpleconfigMessageElement_OSVersion, 4);
    /* apart from the ms bit, which must be 1, do we care what
     *  the other bits are?
     */
    *buf++ = 0x80;      /* MSbit must be set to 1 */
    *buf++ = 0;
    *buf++ = 0;
    *buf++ = 0;
    /* add service type of STA */
    if (serviceType > 1)
    {
        *buf++ = 0x0;
        *buf++ = 0x3;
        *buf++ = 0x7f;
        *buf++ = serviceType;
    }

    /* Now add message header */
    TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Msg
        );
    return TotalLength;
}


int simpleconfigHandleM2(
    void *inbuf,        /* excludes the Eap header */
    int inlen
    )
{
    int line =0xdeadbeef;
    unsigned char *buf = inbuf;
    int buflen = inlen;
    int Offset = 0;
    while (buflen > 4) {
        simpleconfigMsgDump1("simpleconfig Handle M2\n", Offset, buf, buflen);
        int elemid;
        int elemlen;
        unsigned char *elemstart = buf;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if (elemlen > buflen) {
            printf("elemlen  > buflen , %d,%d\n", elemlen, buflen);
            goto BadElem;
        }
        switch(elemid) {
            case simpleconfigMessageElement_MessageType:        /* 1 byte */
            {
                if (elemlen != 1) 
                {
                    printf("elemlen != 1\n");
                    goto BadElem;
                }
                /* TODO: support M2D messages.
                 * Would also require supporting fragmentation...
                 */
                if (buf[0] == simpleconfigMessageType_M2D) {
                    debugf("simpleconfigHandleM2: M2D not supported.\n");
                    return 0x99;
                }
                if (buf[0] != simpleconfigMessageType_M2) 
                {
                    printf("buf[0] != M2\n");
                    goto BadElem;
                }
            break;
            }
            case simpleconfigMessageElement_EnrolleeNonce:      /* 16 B */
                if (elemlen != SIMPLECONFIG_NONCE_SIZE) 
                {
                    printf("elemlen != NONCE SIZE\n");
                    goto BadElem;
                }
                if (memcmp(buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE)) 
                {
                    printf("The NONCE is to ours\n");
                    goto BadElem;
                }
            break;
            case simpleconfigMessageElement_RegistrarNonce:     /* 16 B */
                if (elemlen != SIMPLECONFIG_NONCE_SIZE) 
                {
                    printf("Registrar nonce size != 16\n");
                    goto BadElem;
                }
                memcpy(simpleconfigS.RegistrarNonce, buf, SIMPLECONFIG_NONCE_SIZE);
            break;
            case simpleconfigMessageElement_UuidR:              /* 16 B */
                if (elemlen != SIMPLECONFIG_UIID_SIZE) 
                {
                    printf("UUID is not 16\n");
                    goto BadElem;
                }
                memcpy(simpleconfigS.Results[0].RegistrarUuid, buf, SIMPLECONFIG_UIID_SIZE);
            break;
            case simpleconfigMessageElement_PublicKey:          /* <= 192 B */
                if (elemlen > 192) 
                {
                    printf("Public key is > 192\n");
                    goto BadElem;
                }
                simpleconfigS.RegistrarPublicKeyLen = elemlen;
                memcpy(simpleconfigS.RegistrarPublicKey, buf, elemlen);
            break;
            case simpleconfigMessageElement_AuthenticationTypeFlags: /*2 B*/
                if (elemlen != 2) 
                {
                    printf("Authe Type Flags !=2\n");
                    goto BadElem;
                }
                simpleconfigS.Results[0].AuthenticationTypeFlags =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_EncryptionTypeFlags :  /* 2 bytes */
                if (elemlen != 2) 
                {
                    printf("Encrypt Type Flags !=2\n");
                    goto BadElem;
                }
                simpleconfigS.Results[0].EncryptionTypeFlags =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_ConnectionTypeFlags :  /* 1 byte */
                line = __LINE__;
                if (elemlen != 1) goto BadElem;
                simpleconfigS.Results[0].ConnectionTypeFlags = buf[0];
            break;
            case simpleconfigMessageElement_ConfigMethods       :  /* 2 bytes */
                line = __LINE__;
                if ( elemlen != 2 ) goto BadElem;
                simpleconfigS.Results[0].RegistarConfigMethods =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_Manufacturer        :  /* <= 64 bytes*/
                line = __LINE__;
                if ( elemlen > 64 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_ModelName           :  /* <= 32 bytes*/
                line = __LINE__;
                if ( elemlen > 32 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_ModelNumber         :  /* <= 32 bytes*/
                line = __LINE__;
                if ( elemlen > 32 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_PrimaryDeviceType   :  /* 8 bytes */
                line = __LINE__;
                if ( elemlen != 8 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_DeviceName          :  /* <= 32 bytes*/
                line = __LINE__;
                if ( elemlen > 32 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_RFBands             :  /* 1 byte */
                line = __LINE__;
                if ( elemlen != 1 ) goto BadElem;
                /* We could check that our rf band is supported,
                 * but we probably would not be talking at all if it were not.
                 * For a dual-band device this would get more interesting.
                 */
                /* unused by us */
            break;
            case simpleconfigMessageElement_AssociationState    :  /* 2 bytes */
                line = __LINE__;
                if ( elemlen != 2 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_ConfigurationError  :  /* 2 bytes */
                line = __LINE__;
                if ( elemlen != 2 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_DevicePasswordId    :  /* 2 bytes */
            {
                line = __LINE__;
                if ( elemlen != 2 ) goto BadElem;
                int type = (buf[0] << 8) | buf[1];
                if ( simpleconfigS.PushButtonMethod ) {
                    if ( type != simpleconfigPasswordId_PushButton ) {
                        line = __LINE__;
                        goto BadElem;
                    }
                } else {
                    if ( type == simpleconfigPasswordId_PushButton ) {
                        line = __LINE__;
                        printf("Get PWDID=PBC, while ours is PIN.\n");
                        printf("Ignore it for WAR of Marvell external registrar\n");
                        //goto BadElem;
                    }
                }
            }
            break;
            case simpleconfigMessageElement_OSVersion           :  /* 4 bytes */
                line = __LINE__;
                if ( elemlen != 4 ) goto BadElem;
                /* unused by us */
            break;
            case simpleconfigMessageElement_Authenticator       :  /* 8 bytes */
                line = __LINE__;
                if ( elemlen != 8 ) goto BadElem;
                /* Based on above information, generate keys */
                simpleconfigSessionKeyGen2Post();
                /* Using keys, check authentication of message */
                const unsigned char *vec[6];
                size_t vlen[6];
                unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
                vec[1] = simpleconfigS.M1 + 
                    sizeof(struct simpleconfigEapMessageHeader);
                vlen[1] = simpleconfigS.M1Len -
                    sizeof(struct simpleconfigEapMessageHeader);
                vec[2] = inbuf;
                vlen[2] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    2,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if (memcmp(buf, hash, elemlen)) {
                    debugf("simpleconfig M2 authenticator mismatch\n");
                    debugf("... we have   %02x%02x%02x%02x%02x%02x%02x%02x\n",
                        hash[0], hash[1], hash[2], hash[3],
                        hash[4], hash[5], hash[6], hash[7] );
                    debugf("... they have %02x%02x%02x%02x%02x%02x%02x%02x\n",
                        buf[0], buf[1], buf[2], buf[3],
                        buf[4], buf[5], buf[6], buf[7] );
                    line = __LINE__;
                    if (!simpleconfigS.FakeSuccess)
                        goto BadElem;
                }
            goto Success;
            default:    /* ignore */
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    /* Missing authentication */
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM2 ERROR missing authentication\n");
    return 1;

    Success:
    debugf("simpleconfigHandleM2 OK\n");
    return 0;

    BadElem:
    printf("Line before BadElem = %d\n",line);
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM2 ERROR bad elem\n");
    return 1;

}
int simpleconfigBuildAck( /* returns no. of bytes put in buffer */
    void
    )
{
    unsigned char *buf = simpleconfigS.ACK;
    unsigned char *outbuf = buf;

    printf("simpleconfigBuildACK Entering\n");

    /* Come back later and put in the message header */
    buf += sizeof( struct simpleconfigEapMessageHeader );

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Version, 1 );
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_MessageType, 1 );
    *buf++ = simpleconfigMessageType_Ack;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_EnrolleeNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;


    /* Now add message header */
    int TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Ack
        );
    return TotalLength;
}


/*
 * Enrollee -> Registrar: M3 = Version || N2 || E-Hash1 || E-Hash2 ||
 *                      HMACAuthKey(M2 || M3*)
 */
int simpleconfigBuildM3( /* returns no. of bytes put in buffer */
    void
    )
{
    unsigned char *buf = simpleconfigS.M3;
    unsigned char *outbuf = buf;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];

    printf("simpleconfigBuildM3 Entering\n");

    /* Come back later and put in the message header */
    buf += sizeof( struct simpleconfigEapMessageHeader );

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Version, 1 );
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_MessageType, 1 );
    *buf++ = simpleconfigMessageType_M3;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;


    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_EHash1, 32 );
    memcpy( buf, simpleconfigS.EHash1, 32 );
    buf += 32;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_EHash2, 32 );
    memcpy( buf, simpleconfigS.EHash2, 32 );
    buf += 32;

    // Compute authenticator as hash of M2 received and
    //  M3 built so far.
    unsigned char *hashend = buf;
    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Authenticator, 
        SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    /* reserve vec[0] for internal use */
    vec[1] = simpleconfigS.M2 +
                sizeof(struct simpleconfigEapMessageHeader);
    vlen[1] = simpleconfigS.M2Len -
                sizeof(struct simpleconfigEapMessageHeader);
    vec[2] = simpleconfigS.M3 +
                sizeof(struct simpleconfigEapMessageHeader);
    vlen[2] = hashend - vec[2];
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        2,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    memcpy( buf, hash, SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    buf += SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE;

    /* Now add message header */
    int TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Msg
        );
    return TotalLength;
}

int simpleconfigHandleM4Encrypted(
    void *inbuf, 
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;

    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M4Encrypted", Offset, buf, buflen);
        unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_RSNonce1 :
                if ( elemlen != sizeof(simpleconfigS.RNonce1) )
                    goto BadElem;
                memcpy(simpleconfigS.RNonce1,buf,sizeof(simpleconfigS.RNonce1));
            break;
            case simpleconfigMessageElement_KeyWrapAuthenticator :
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE )
                    goto BadElem;
                vec[1] = inbuf;
                vlen[1] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    1,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) goto BadElem;
            goto Done;
            default:
                /* ignore other */
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    debugf("simpleconfigHandleM4Encrypted ERROR missing authentication");
    return 1;

    Done:
    debugf("simpleconfigHandleM4 OK");
    return 0;

    BadElem:
    debugf("simpleconfigHandleM4Encrypted ERROR bad elem");
    return 1;
}

/*
 * Enrollee  Registrar: M4 = Version || N1 || R-Hash1 || R-Hash2 ||
 *                     ENCKeyWrapKey(R-S1) || HMACAuthKey (M3 || M4*)
 */
int simpleconfigHandleM4(
    void *inbuf,        /* excludes the Eap header */
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;

    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M4", Offset, buf, buflen);
        int elemid;
        int elemlen;
        unsigned char *elemstart = buf;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_MessageType:        /* 1 byte */
            {
                if ( elemlen != 1 ) goto BadElem;
                if ( buf[0] != simpleconfigMessageType_M4 ) goto BadElem;
            }
            break;
            case simpleconfigMessageElement_EnrolleeNonce:      /* 16 B */
                if ( elemlen != SIMPLECONFIG_NONCE_SIZE ) goto BadElem;
                if ( memcmp(buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE) ) goto BadElem;
            break;
            case simpleconfigMessageElement_RHash1              :  /* 32 bytes */
                if ( elemlen != 32 ) goto BadElem;
                memcpy(simpleconfigS.RHash1,buf,sizeof(simpleconfigS.RHash1));
            break;
            case simpleconfigMessageElement_RHash2              :  /* 32 bytes */
                if ( elemlen != 32 ) goto BadElem;
                memcpy(simpleconfigS.RHash2,buf,sizeof(simpleconfigS.RHash2));
            break;
            case simpleconfigMessageElement_EncryptedSettings   :
                if ( elemlen <= SIMPLECONFIG_IV_SIZE ) goto BadElem;
                if ( elemlen >= sizeof(simpleconfigS.M4e) ) goto BadElem;
                memset(simpleconfigS.M4e, 0, sizeof(simpleconfigS.M4e));
                simpleconfigS.M4eLen = simpleconfigUnWrap(
                    buf,
                    elemlen,
                    simpleconfigS.M4e
                    );
                if ( simpleconfigHandleM4Encrypted( 
                        simpleconfigS.M4e,
                        simpleconfigS.M4eLen ) ) goto BadElem;
            break;
            case simpleconfigMessageElement_Authenticator       :  /* 8 bytes */
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE ) 
                    goto BadElem;
                vec[1] = simpleconfigS.M3 +
                    sizeof(struct simpleconfigEapMessageHeader);
                vlen[1] = simpleconfigS.M3Len -
                    sizeof(struct simpleconfigEapMessageHeader);
                vec[2] = inbuf;
                vlen[2] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    2,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) {
                    debugf("simpleconfig M4 authenticator mismatch");
                    debugf("... we have   %02x%02x%02x%02x%02x%02x%02x%02x",
                        hash[0], hash[1], hash[2], hash[3],
                        hash[4], hash[5], hash[6], hash[7] );
                    debugf("... they have %02x%02x%02x%02x%02x%02x%02x%02x",
                        buf[0], buf[1], buf[2], buf[3],
                        buf[4], buf[5], buf[6], buf[7] );
                    if ( ! simpleconfigS.FakeSuccess ) 
                        goto BadElem;
                }
            goto Done;
            default:
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    debugf("simpleconfigHandleM4 ERROR missing authentication");
    return 1;

    Done:
    /* Validate received RHash1 */
    vec[1] = simpleconfigS.RNonce1;
    vlen[1] = sizeof(simpleconfigS.RNonce1);
    vec[2] = simpleconfigS.Psk1;
    vlen[2] = sizeof(simpleconfigS.Psk1);
    vec[3] = simpleconfigS.OurPublicKey;
    vlen[3] = simpleconfigS.OurPublicKeyLen;
    vec[4] = simpleconfigS.RegistrarPublicKey;
    vlen[4] = simpleconfigS.RegistrarPublicKeyLen;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        4,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    if ( memcmp(hash, simpleconfigS.RHash1, sizeof(hash)) )
        goto BadElem;
    debugf("simpleconfigHandleM4 OK");
    return 0;
    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM4 ERROR bad elem");
    return 1;
}

/*
 * Enrollee  Registrar: M5 = Version || N2 || ENCKeyWrapKey(E-S1) ||
 *                     HMACAuthKey (M4 || M5*)
 */
int simpleconfigBuildM5( /* returns no. of bytes put in buffer */
    void *outbuf        /* must be big enough */
    )
{
    unsigned char *buf = outbuf;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];

    /* Come back later and put in the message header */
    buf += sizeof( struct simpleconfigEapMessageHeader );

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Version, 1 );
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_MessageType, 1 );
    *buf++ = simpleconfigMessageType_M5;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;

    /* Here for the "encrypted settings" */
    unsigned char *settingsHdr = buf;
    buf += 4;   /* come back and do it later */
    /* Here for the "encrypted settings" content: */
    unsigned char *settingsContent = buf;
    /* Here is the payload to be encrypted: */
    unsigned char *dataToEncrypt = simpleconfigS.M5e;
    unsigned char *plain = dataToEncrypt;
    plain = simpleconfigBuildMessageElementHeader( plain,
        simpleconfigMessageElement_ESNonce1, SIMPLECONFIG_NONCE_SIZE );
    memcpy( plain, simpleconfigS.ENonce1, SIMPLECONFIG_NONCE_SIZE );
    plain += SIMPLECONFIG_NONCE_SIZE;
    /* with authenticator at the end: */
    unsigned char *endDataToHash = plain;
    plain = simpleconfigBuildMessageElementHeader( plain,
        simpleconfigMessageElement_KeyWrapAuthenticator, 
        SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    vec[1] = dataToEncrypt;
    vlen[1] = endDataToHash - dataToEncrypt;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        1,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    memcpy( plain, hash, SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    plain += SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE;
    simpleconfigS.M5eLen = plain-dataToEncrypt;
    /* Now wrap that (encrypt it) */
    int ncrypt = 0;
    ncrypt = simpleconfigWrap(
        dataToEncrypt,
        plain-dataToEncrypt,
        buf
        );
    buf += ncrypt;
    /* Now that we know length, fill header for encrypted settings */
    simpleconfigBuildMessageElementHeader( settingsHdr,
        simpleconfigMessageElement_EncryptedSettings, buf-settingsContent );

    // Compute authenticator as hash of M4 received and
    //  M5 built so far.
    unsigned char *hashend = buf;
    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Authenticator, 
        SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    /* reserve vec[0] for internal use */
    vec[1] = simpleconfigS.M4 +
                    sizeof(struct simpleconfigEapMessageHeader);
    vlen[1] = simpleconfigS.M4Len -
                    sizeof(struct simpleconfigEapMessageHeader);
    vec[2] = simpleconfigS.M5 +
                    sizeof(struct simpleconfigEapMessageHeader);
    vlen[2] = hashend - vec[2];
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        2,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    memcpy( buf, hash, SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    buf += SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE;

    /* Now add message header */
    int TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Msg
        );
    return TotalLength;
}

int simpleconfigHandleM6Encrypted(
    void *inbuf, 
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;

    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M6Encrypted", Offset, buf, buflen);
        unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_RSNonce2 :
                if ( elemlen != sizeof(simpleconfigS.RNonce2) )
                    goto BadElem;
                memcpy(simpleconfigS.RNonce2,buf,sizeof(simpleconfigS.RNonce1));
            break;
            case simpleconfigMessageElement_KeyWrapAuthenticator :
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE )
                    goto BadElem;
                vec[1] = inbuf;
                vlen[1] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    1,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) goto BadElem;
            goto Done;
            default:
                /* ignore other */
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM6Encrypted ERROR missing authentication");
    return 1;

    Done:
    debugf("simpleconfigHandleM6 OK");
    return 0;

    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM6Encrypted ERROR bad elem");
    return 1;
}

/*
 * Enrollee  Registrar: M6 = Version || N1 || ENCKeyWrapKey(R-S2) ||
 *                   HMACAuthKey (M5 || M6*)
 */
int simpleconfigHandleM6(
    void *inbuf,        /* excludes the Eap header */
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;
    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M6", Offset, buf, buflen);
        unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_MessageType:        /* 1 byte */
            {
                if ( elemlen != 1 ) goto BadElem;
                if ( buf[0] != simpleconfigMessageType_M6 ) goto BadElem;
            }
            break;
            case simpleconfigMessageElement_EnrolleeNonce:      /* 16 B */
                if ( elemlen != SIMPLECONFIG_NONCE_SIZE ) goto BadElem;
                if ( memcmp(buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE) ) goto BadElem;
            break;
            case simpleconfigMessageElement_EncryptedSettings   :
                if ( elemlen <= SIMPLECONFIG_IV_SIZE ) goto BadElem;
                if ( elemlen >= sizeof(simpleconfigS.M6e) ) goto BadElem;
                simpleconfigS.M6eLen = simpleconfigUnWrap(
                    buf,
                    elemlen,
                    simpleconfigS.M6e
                    );
                if ( simpleconfigHandleM6Encrypted( 
                        simpleconfigS.M6e,
                        simpleconfigS.M6eLen ) ) goto BadElem;
            break;
            case simpleconfigMessageElement_Authenticator       :  /* 8 bytes */
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE ) 
                    goto BadElem;
                vec[1] = simpleconfigS.M5 +
                    sizeof(struct simpleconfigEapMessageHeader);
                vlen[1] = simpleconfigS.M5Len -
                    sizeof(struct simpleconfigEapMessageHeader);
                vec[2] = inbuf;
                vlen[2] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    2,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) {
                    debugf("simpleconfig M6 authenticator mismatch");
                    debugf("... we have   %02x%02x%02x%02x%02x%02x%02x%02x",
                        hash[0], hash[1], hash[2], hash[3],
                        hash[4], hash[5], hash[6], hash[7] );
                    debugf("... they have %02x%02x%02x%02x%02x%02x%02x%02x",
                        buf[0], buf[1], buf[2], buf[3],
                        buf[4], buf[5], buf[6], buf[7] );
                    if ( ! simpleconfigS.FakeSuccess ) 
                        goto BadElem;
                }
            goto Done;
            default:
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM6 ERROR missing authentication");
    return 1;
    Done:
    /* Validate received RHash2 */
    vec[1] = simpleconfigS.RNonce2;
    vlen[1] = sizeof(simpleconfigS.RNonce2);
    vec[2] = simpleconfigS.Psk2;
    vlen[2] = sizeof(simpleconfigS.Psk2);
    vec[3] = simpleconfigS.OurPublicKey;
    vlen[3] = simpleconfigS.OurPublicKeyLen;
    vec[4] = simpleconfigS.RegistrarPublicKey;
    vlen[4] = simpleconfigS.RegistrarPublicKeyLen;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        4,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    if ( memcmp(hash, simpleconfigS.RHash2, sizeof(hash)) )
        goto BadElem;
    debugf("simpleconfigHandleM6 OK");
    return 0;
    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM4 ERROR bad elem");
    return 1;
}

/*
 * Enrollee  Registrar: M7 = Version || N2|| ENCKeyWrapKey(E-S2 [||ConfigData]) ||
 *                     HMACAuthKey (M6 || M7*)
 */
int simpleconfigBuildM7( /* returns no. of bytes put in buffer */
    void *outbuf        /* must be big enough */
    )
{
    unsigned char *buf = outbuf;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];

    /* Come back later and put in the message header */
    buf += sizeof( struct simpleconfigEapMessageHeader );

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Version, 1 );
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_MessageType, 1 );
    *buf++ = simpleconfigMessageType_M7;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;

    /* Here for the "encrypted settings" */
    unsigned char *settingsHdr = buf;
    buf += 4;   /* come back and do it later */
    /* Here for the "encrypted settings" content: */
    unsigned char *settingsContent = buf;
    /* Here is the payload to be encrypted: */
    unsigned char *dataToEncrypt = simpleconfigS.M7e;
    unsigned char *plain = dataToEncrypt;
    plain = simpleconfigBuildMessageElementHeader( plain,
        simpleconfigMessageElement_ESNonce2, SIMPLECONFIG_NONCE_SIZE );
    memcpy( plain, simpleconfigS.ENonce2, SIMPLECONFIG_NONCE_SIZE );
    plain += SIMPLECONFIG_NONCE_SIZE;
    /* with authenticator at the end: */
    unsigned char *endDataToHash = plain;
    plain = simpleconfigBuildMessageElementHeader( plain,
        simpleconfigMessageElement_KeyWrapAuthenticator, 
        SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    vec[1] = dataToEncrypt;
    vlen[1] = endDataToHash - dataToEncrypt;
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        1,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    memcpy( plain, hash, SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    plain += SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE;
    simpleconfigS.M7eLen = plain-dataToEncrypt;
    /* Now wrap that (encrypt it) */
    int ncrypt = 0;
    ncrypt = simpleconfigWrap(
        dataToEncrypt,
        plain-dataToEncrypt,
        buf
        );
    buf += ncrypt;
    /* Now that we know length, fill header for encrypted settings */
    simpleconfigBuildMessageElementHeader( settingsHdr,
        simpleconfigMessageElement_EncryptedSettings, buf-settingsContent );

    // Compute authenticator as hash of M6 received and
    //  M7 built so far.
    unsigned char *hashend = buf;
    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Authenticator, 
        SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    /* reserve vec[0] for internal use */
    vec[1] = simpleconfigS.M6 +
                    sizeof(struct simpleconfigEapMessageHeader);
    vlen[1] = simpleconfigS.M6Len -
                    sizeof(struct simpleconfigEapMessageHeader);
    vec[2] = simpleconfigS.M7 +
                    sizeof(struct simpleconfigEapMessageHeader);
    vlen[2] = hashend - vec[2];
    simpleconfigHmacSHA256(
        simpleconfigS.AuthKey,
        SIMPLECONFIG_AUTHKEY_SIZE ,
        2,
        /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
        * so that vec[0] and vlen[0] can be scribbled over.
        */
        vec,
        vlen,
        hash
        );
    memcpy( buf, hash, SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE );
    buf += SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE;

    /* Now add message header */
    int TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Msg
        );
    return TotalLength;
}

int simpleconfigHandleM8Credential(
    void *inbuf, 
    int inlen,
    int cidx //Credential buffer index
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    int Offset = 0;

    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M8Credential", Offset, buf, buflen);
        // unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            // Required: SSID, AuthenticationType, EncryptionType, 
            // NetworkKey, MacAddress, 
            // Should we do some optional as well?
            case simpleconfigMessageElement_Ssid :          /*required*/
                if ( elemlen > WLAN_SSID_LEN ) goto BadElem;
                //sprncpy(simpleconfigS.Results.Ssid, 
                //    sizeof(simpleconfigS.Results.Ssid),
                //    buf, elemlen );
                //strncpy(simpleconfigS.Results.Ssid, buf,
                //      sizeof(simpleconfigS.Results.Ssid));
                strncpy(simpleconfigS.Results[cidx].Ssid, buf, elemlen);
            break;
            case simpleconfigMessageElement_AuthenticationType :   /*required*/
                if ( elemlen != 2 ) goto BadElem;
                simpleconfigS.Results[cidx].AuthenticationType =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_AuthenticationTypeFlags : /*opt*/
                if ( elemlen != 2 ) goto BadElem;
                simpleconfigS.Results[cidx].AuthenticationTypeFlags =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_EncryptionType :   /*required*/
                if ( elemlen != 2 ) goto BadElem;
                simpleconfigS.Results[cidx].EncryptionType =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_EncryptionTypeFlags :  /*opt*/
                if ( elemlen != 2 ) goto BadElem;
                simpleconfigS.Results[cidx].EncryptionTypeFlags =
                    (buf[0] << 8) | buf[1];
            break;
            case simpleconfigMessageElement_NetworkKey :   /*required*/
                if ( elemlen > 64 ) goto BadElem;
                memcpy( simpleconfigS.Results[cidx].NetworkKey, buf, elemlen );
                simpleconfigS.Results[cidx].NetworkKeyLen = elemlen;
            break;
            case simpleconfigMessageElement_NetworkKeyIndex :   /*required*/
                if ( elemlen > 1 ) goto BadElem;
                simpleconfigS.Results[cidx].WepTransmitKeyIdx = buf[0]-1;
            break;
            case simpleconfigMessageElement_MacAddress : /*required*/
                /* what is this for? should not be necessary? */
            break;
            case simpleconfigMessageElement_ApChannel : /* opt */
                simpleconfigS.Results[cidx].ApChannel = 
                    (buf[0] << 8) | buf[1];
            break;
            /* MORE? */
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    debugf("simpleconfigHandleM8Credential OK\n");
    return 0;

    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM8Credential ERROR bad elem\n");
    return 1;

}

int simpleconfigHandleM8Encrypted(
    void *inbuf, 
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;
    simpleconfigS.ResultCnt = 0;

    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M8Encrypted", Offset, buf, buflen);
        unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_Credential :
                /* This is a compound element */
                if (simpleconfigS.ResultCnt >= sizeof(simpleconfigS.Results)/sizeof(simpleconfigS.Results[0]))
                    break;
                if ( simpleconfigHandleM8Credential( buf, elemlen, simpleconfigS.ResultCnt) )
                    goto BadElem;
                simpleconfigS.ResultCnt++;
            break;
            case simpleconfigMessageElement_KeyWrapAuthenticator :
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE )
                    goto BadElem;
                vec[1] = inbuf;
                vlen[1] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    1,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) goto BadElem;
            goto Done;
            default:
                /* ignore other */
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM8Encrypted ERROR missing authentication\n");
    return 1;

    Done:
    debugf("simpleconfigHandleM8Encrypted OK\n");
    return 0;

    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM8Encrypted ERROR bad elem\n");
    return 1;
}

/*
 * Enrollee  Registrar: M8 = Version || N1 || [ ENCKeyWrapKey(ConfigData) ] ||
 *                    HMACAuthKey (M7 || M8*)
 */
int simpleconfigHandleM8(
    void *inbuf,        /* excludes the Eap header */
    int inlen
    )
{
    unsigned char *buf = inbuf;
    int buflen = inlen;
    const unsigned char *vec[6];
    size_t vlen[6];
    unsigned char hash[SIMPLECONFIG_SHA256_OUTPUT_SIZE];
    int Offset = 0;
    while ( buflen > 4 ) {
        simpleconfigMsgDump1("simpleconfig Handle M8", Offset, buf, buflen);
        unsigned char *elemstart = buf;
        int elemid;
        int elemlen;
        elemid = (*buf++) << 8;
        elemid |= *buf++;
        elemlen = (*buf++) << 8;
        elemlen |= *buf++;
        buflen -= 4;
        if ( elemlen > buflen ) {
            goto BadElem;
        }
        switch( elemid ) {
            case simpleconfigMessageElement_MessageType:        /* 1 byte */
            {
                if ( elemlen != 1 ) goto BadElem;
                if ( buf[0] != simpleconfigMessageType_M8) goto BadElem;
            }
            break;
            case simpleconfigMessageElement_EnrolleeNonce:      /* 16 B */
                if ( elemlen != SIMPLECONFIG_NONCE_SIZE ) goto BadElem;
                if ( memcmp(buf, simpleconfigS.OurNonce, 
                    SIMPLECONFIG_NONCE_SIZE) ) goto BadElem;
            break;
            case simpleconfigMessageElement_EncryptedSettings   :
                if ( elemlen <= SIMPLECONFIG_IV_SIZE ) goto BadElem;
                if ( elemlen >= sizeof(simpleconfigS.M8e) ) goto BadElem;
                simpleconfigS.M8eLen = simpleconfigUnWrap(
                    buf,
                    elemlen,
                    simpleconfigS.M8e
                    );
                if ( simpleconfigHandleM8Encrypted( 
                        simpleconfigS.M8e,
                        simpleconfigS.M8eLen ) ) goto BadElem;
            break;
            case simpleconfigMessageElement_Authenticator :  /* 8 bytes */
                if ( elemlen != SIMPLECONFIG_KEYWRAPAUTHENTICATOR_SIZE ) 
                    goto BadElem;
                vec[1] = simpleconfigS.M7 +
                    sizeof(struct simpleconfigEapMessageHeader);
                vlen[1] = simpleconfigS.M7Len -
                    sizeof(struct simpleconfigEapMessageHeader);
                vec[2] = inbuf;
                vlen[2] = elemstart - (unsigned char *)inbuf;
                simpleconfigHmacSHA256(
                    simpleconfigS.AuthKey,
                    SIMPLECONFIG_AUTHKEY_SIZE ,
                    2,
                    /* IMPORTANT: leave vec[0]/vlen[0] unused (begin at [1])
                    * so that vec[0] and vlen[0] can be scribbled over.
                    */
                    vec,
                    vlen,
                    hash
                    );
                if ( memcmp(buf, hash, elemlen) ) {
                    debugf("simpleconfig M8 authenticator mismatch");
                    debugf("... we have   %02x%02x%02x%02x%02x%02x%02x%02x",
                        hash[0], hash[1], hash[2], hash[3],
                        hash[4], hash[5], hash[6], hash[7] );
                    debugf("... they have %02x%02x%02x%02x%02x%02x%02x%02x",
                        buf[0], buf[1], buf[2], buf[3],
                        buf[4], buf[5], buf[6], buf[7] );
                    if ( ! simpleconfigS.FakeSuccess ) 
                        goto BadElem;
                }
            goto Done;
            default:
            break;
        }
        buflen -= elemlen;
        buf += elemlen;
        Offset += 4+elemlen;
    }
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM8 ERROR missing authentication");
    return 1;

    Done:
    debugf("simpleconfigHandleM8 OK");
    return 0;

    BadElem:
    simpleconfigS.NErrors++;
    debugf("simpleconfigHandleM8 ERROR bad elem");
    return 1;
}

/*
 * 
 */
int simpleconfigBuildDonePkt( /* returns no. of bytes put in buffer */
    void *outbuf        /* must be big enough */
    )
{
    unsigned char *buf = outbuf;

    /* Come back later and put in the message header */
    buf += sizeof( struct simpleconfigEapMessageHeader );

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_Version, 1 );
    *buf++ = simpleconfigVersionEncoding;      /* per intel reference */

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_MessageType, 1 );
    *buf++ = simpleconfigMessageType_Done;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_EnrolleeNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.OurNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;

    buf = simpleconfigBuildMessageElementHeader( buf,
        simpleconfigMessageElement_RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    memcpy( buf, simpleconfigS.RegistrarNonce, SIMPLECONFIG_NONCE_SIZE );
    buf += SIMPLECONFIG_NONCE_SIZE;

    /* Now add message header */
    int TotalLength = buf-(unsigned char *)outbuf;
    simpleconfigBuildSimpleConfigHeader(
        outbuf,
        TotalLength,
        simpleconfigS.LastEapIdentifier,
        simpleconfigSimpleConfigOpcode_Done
        );
    return TotalLength;
}

void simpleconfigTimerTaskHandler(struct wsc_drv *wsc_drv)
{
    const char *StateName = simpleconfigStateNames[simpleconfigS.State];

    if (StateName == NULL)
        StateName = "";

    debugf("simpleconfigTimerTaskHandler state %d %s\n", 
            simpleconfigS.State, StateName);

    if (simpleconfigS.Failure)
    {
        debugf("simpleconfigTimerTaskHandler unexpected failure state\n");
        return;
    }

    /* TODO: add timer expiration check */
#if 0
    //unsigned SecondsElapsed = mainthreadSecondsSinceBoot() -
    //        simpleconfigS.StartSeconds;

    if ( SecondsElapsed >= 120 ) {
        /* WPS imposes 120 second limitation */
        printf("simpleconfig EXCEEDED 120 seconds... STOP!\n");
        simpleconfigCancelCurrent();
        simpleconfigS.LastError = LANG_Menu_Setup_wlan_autoconfig_timeout;
        simpleconfigS.LastErrMsg[0] = 0;
        simpleconfigS.Failure = 1;
        return;
    }
#endif

    switch ( simpleconfigS.State )
    {
        case simpleconfigState_eStart:
            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;

            /* Send EAPOL Start message ... first thing we do */
            memset(&simpleconfigS.Results, 0, sizeof(simpleconfigS.Results));

            /* Send simple start message to AP, which will 
             * make requests to which we must reply to.
             */
            simpleconfigStartSend();
        goto SetTimer;
        case simpleconfigState_eIdentity:
            /* Got identity message; send identity message back */
            /* This will be sent by wpa_supplicant */
            //if (++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES)
            //    goto StartOver;
            //simpleconfigIdentityResponseSend();
        goto SetTimer;
        case simpleconfigState_eM1:
            if (simpleconfigS.NTries == 0)
            {
                memset(simpleconfigS.M1, 0, sizeof(simpleconfigS.M1));
                simpleconfigS.M1Len = simpleconfigBuildM1(wsc_drv,simpleconfigS.M1, wsc_drv->serviceType);
                simpleconfigMsgDump( "simpleconfig Send M1",
                    simpleconfigS.M1,
                    simpleconfigS.M1Len
                );
            }

            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;

            simpleconfigPacketSend(wsc_drv, "M1",
                simpleconfigS.M1,
                simpleconfigS.M1Len
                );
        goto SetTimer;
        case simpleconfigState_eM3:
	    if(connect_start_time == 0)
	        connect_start_time = time(NULL);
            if ( simpleconfigS.NTries == 0 ) {
                memset( simpleconfigS.M3, 0, sizeof(simpleconfigS.M3) );
                simpleconfigS.M3Len = simpleconfigBuildM3();
                simpleconfigMsgDump( "simpleconfig Send M3",
                    simpleconfigS.M3,
                    simpleconfigS.M3Len
                    );
            }
            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;
            simpleconfigPacketSend(wsc_drv, "M3",
                simpleconfigS.M3,
                simpleconfigS.M3Len
                );
        goto SetTimer;
        case simpleconfigState_eM5:
            if ( simpleconfigS.NTries == 0 ) {
                memset( simpleconfigS.M5, 0, sizeof(simpleconfigS.M5) );
                simpleconfigS.M5Len = simpleconfigBuildM5(
                    simpleconfigS.M5 );
                simpleconfigMsgDump( "simpleconfig Send M5",
                    simpleconfigS.M5,
                    simpleconfigS.M5Len
                    );
            }
            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;
            simpleconfigPacketSend(wsc_drv, "M5",
                simpleconfigS.M5,
                simpleconfigS.M5Len
                );
        goto SetTimer;
        case simpleconfigState_eM7:
            if ( simpleconfigS.NTries == 0 ) {
                memset( simpleconfigS.M7, 0, sizeof(simpleconfigS.M7) );
                simpleconfigS.M7Len = simpleconfigBuildM7(
                    simpleconfigS.M7 );
                simpleconfigMsgDump( "simpleconfig Send M7",
                    simpleconfigS.M7,
                    simpleconfigS.M7Len
                    );
            }
            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;
            simpleconfigPacketSend(wsc_drv, "M7",
                simpleconfigS.M7,
                simpleconfigS.M7Len
                );
        goto SetTimer;
        case simpleconfigState_eDone:
            if ( ++simpleconfigS.NTries > SIMPLECONFIG_MAX_TRIES ) 
                goto StartOver;
            simpleconfigS.DonePktLen = simpleconfigBuildDonePkt(
                simpleconfigS.DonePkt );
            simpleconfigMsgDump( "simpleconfig Send DonePkt",
                simpleconfigS.DonePkt,
                simpleconfigS.DonePktLen
                );
            simpleconfigPacketSend(wsc_drv, "Done",
                simpleconfigS.DonePkt,
                simpleconfigS.DonePktLen
                );
        goto SetTimer;
        default:
            debugf("simpleconfigTimerTaskHandler: unexpected state %d",
                simpleconfigS.State );
        break;
    }
    return;

    StartOver:
    debugf("simpleconfigTimerTaskHandler StartOver");
    /* TODO: */
#if 0
    memset( simpleconfigS.NackPkt, 0, sizeof(simpleconfigS.NackPkt));
    simpleconfigS.NackPktLen = simpleconfigBuildNack( 
        simpleconfigS.NackPkt,
        simpleconfigConfigurationError_MessageTimeout /* ? */
        );
    simpleconfigPacketSend(wsc_drv, "NackPkt",
        simpleconfigS.NackPkt,
        simpleconfigS.NackPktLen
        );
    simpleconfigStartOver();
#endif
    return;

    SetTimer:

    /* TODO: Send the EAPOL frame */

#if 0
    debugf("simpleconfigTimerTaskHandler Waiting");
    pollerTaskLongTimeoutRelSeconds(
        &simpleconfigS.Timer,
        SIMPLECONFIG_REPLY_TIMEOUT_SECONDS
        );
#endif
    return;
}

void handlemsg(struct wsc_drv *wsc_drv, char *buf)
{
    int i;
#if 0
    /* Copy packet to state structure for easier access and 
     *  possibly to ease debugging etc.
     */
    memset(simpleconfigS.RcvdPkt, 0, sizeof(simpleconfigS.RcvdPkt));
    simpleconfigS.RcvdPktLen = lwipPbufUnpack(
        p, simpleconfigS.RcvdPkt, sizeof(simpleconfigS.RcvdPkt) );
    pbuf_free(p);
    if ( simpleconfigS.RcvdPktLen <= 0 ) {
        debugf("simpleconfigRecv: lwipPbufUnpack error!");
        return;
    }
    struct simpleconfigEapMessageHeader *hdr = 
        (void *)simpleconfigS.RcvdPkt;
    /* sanity check the header */
    if ( hdr->EthernetProtocol != htons(ETH_P_EAPOL) ) goto Insane;
    if ( simpleconfigCompareEthernetAddress(
        hdr->EthernetSrcAddr, simpleconfigS.ApAddr ) ) goto Insane;
    if ( simpleconfigCompareEthernetAddress(
        hdr->EthernetDestAddr, simpleconfigS.OurAddr ) ) goto Insane;
    // ? check hdr->EapolProtocolVersion ?
    if ( hdr->EapolPacketType != simpleconfigEapolPacketType_EAP ) 
        goto Insane;
    unsigned EapolPacketBodyLength = ntohs(hdr->EapolPacketBodyLength);
    if ( EapolPacketBodyLength > simpleconfigS.RcvdPktLen - simpleconfigEapStartOffset )
        goto Insane;
    unsigned EapLength = ntohs(hdr->EapLength);
    if ( EapLength != EapolPacketBodyLength ) goto Insane;
    debugf("simpleconfigRecv: EapCode==%d EapLength=%d", hdr->EapCode, EapLength);
#endif

    /* EapLength includes the EAP header but not of course the 
     * stuff before the EAP header. The message is after the EAP
     * header and is a sequence of information elements,
     * each ie of the form: (type,len,body).
     */
    //int PktLen = EapLength + simpleconfigEapStartOffset;
    //int MsgLen = PktLen - sizeof(*hdr);
    //void *Msg = (void *)(hdr+1);

    struct simpleconfigEapMessageHeader *hdr = 
        (struct simpleconfigEapMessageHeader *)buf;

    int PktLen = ntohs(hdr->EapLength);
    int MsgLen = PktLen - sizeof(*hdr);
    void *Msg = (void *)(hdr+1);

    /* Handle recv of retransmitted packets */
    switch ( simpleconfigS.State ) {
    case simpleconfigState_eOff :
        break;
    case simpleconfigState_eStart :
        break;
    case simpleconfigState_eIdentity :
        break;
    case simpleconfigState_eM1 : {
        if ( hdr->EapType == simpleconfigEapType_Identity ) {
            /* Ignore retransmit */
            debugf("simpleconfigRecv: ignoring recv of retransmitted identity");
            return;
        }
    }
        break;
    case simpleconfigState_eM2 :  
        /* transitory state */
        break;
    case simpleconfigState_eM3 :  {
        if ( PktLen == simpleconfigS.M2Len &&
                !memcmp( hdr, simpleconfigS.M2, simpleconfigS.M2Len ) ) {
            /* Ignore retransmit */
            debugf("simpleconfigRecv: ignoring recv of retransmitted M2");
            return;
        }
    }
        break;
    case simpleconfigState_eM4 :
        /* transitory state */
        break;
    case simpleconfigState_eM5 : {
        if ( PktLen == simpleconfigS.M4Len &&
                !memcmp( hdr, simpleconfigS.M4, simpleconfigS.M4Len ) ) {
            /* Ignore retransmit */
            debugf("simpleconfigRecv: ignoring recv of retransmitted M4");
            return;
        }
    }
        break;
    case simpleconfigState_eM6 :
        /* transitory state */
        break;
    case simpleconfigState_eM7 : {
        if ( PktLen == simpleconfigS.M6Len &&
                !memcmp( hdr, simpleconfigS.M6, simpleconfigS.M6Len ) ) {
            /* Ignore retransmit */
            debugf("simpleconfigRecv: ignoring recv of retransmitted M6");
            return;
        }
    }
        break;
    case simpleconfigState_eM8 :
        /* transitory state */
        break;
    case simpleconfigState_eDone : {
        if ( PktLen == simpleconfigS.M8Len &&
                !memcmp( hdr, simpleconfigS.M8, simpleconfigS.M8Len ) ) {
            /* Ignore retransmit */
            debugf("simpleconfigRecv: ignoring recv of retransmitted M8");
            return;
        }
    }
        break;
    }

    if (hdr->EapCode == simpleconfigEapCode_Failure) {
        /* EapCode 4 is "failure" which is normal condition at the end.
         *      Now we are really done... unless there is some error.
         *      Configure the current wlan profile w/ simpleconfig off,
         *      which will cause us to be disabled.
        */
        //if (simpleconfigMaybeDone())
        {
            printf("SimpleconfigMaybeDone\n");

            if (simpleconfigS.IsInit == 0)
            {
                if (wsc_drv->l2 != NULL)
                {
                    l2_packet_deinit(wsc_drv->l2);
                    wsc_drv->l2 = NULL;
                }
                
                if (wsc_drv->l2_br != NULL)
                {
                    l2_packet_deinit(wsc_drv->l2_br);
                    wsc_drv->l2_br = NULL;
                }
    
                if (set80211param(wsc_drv, IEEE80211_PARAM_ROAMING, 1, 1) < 0) {
                    wsc_printf(WSC_DBG_INIT, "%s: failed to set wpa_supplicant-based "
                            "roaming\n", __FUNCTION__);
                }

                #if 0
                /* Invoke hostapd, wsccmd will do this */
                wsc_drv->hostapd_pid = wsc_invoke_hostapd("/etc/ath", "hostapd.conf");
                #endif

                if (wsc_drv->wds_ap_ifname != NULL)
                {
                    /* Invoke wsccmd */
                    wsc_drv->wsccmd_pid = wsc_invoke_wsccmd("/etc/ath/", 1);

                    /* Delay to wait wsccmd brings up hostapd */
                    sleep(wsc_drv->sup_delay);
                }
                wps_driver_zydas_wps_filter(wsc_drv,0);
                if (zydas_set_param(wsc_drv, 1, 0, 1) < 0) //Let driver scan
                    printf("Set Roamming failed\n");
                /* Invoke wpa_supplicant */
                wsc_drv->wpa_supplicant_pid = 
                    wsc_invoke_wpa_supplicant(wsc_drv->ifname, "", "wpa_supplicant.conf");

                /* Close wps_enrollee when all things done */
                if (wsc_drv->alive == 0)
                    exit(0);
            }
            goto Restart;
        }

        return;
    }
    if (hdr->EapCode != simpleconfigEapCode_Request)
        goto Insane;

    simpleconfigS.LastEapIdentifier = hdr->EapIdentifier;
    if (hdr->EapType == simpleconfigEapType_Identity) { /* Request-identity */
            switch( simpleconfigS.State ) {
                case simpleconfigState_eOff:
                    // should not happen but just in case
                return;
                case simpleconfigState_eStart:
                    simpleconfigNewState(simpleconfigState_eIdentity);
                break;
                case simpleconfigState_eIdentity:
                    // ignore redundant identity message
                break;
                default:
                #if 0   // should be? maybe...
                goto BadMsg;
                #endif
                break;
            }
        return;
    }

    if (hdr->EapType != simpleconfigEapType_Expanded)
    {
        goto Insane; 
    }
    if (hdr->EapVendorId[0] != (0xFF & (simpleconfigHeaderVendorId>>16)))
    {
        goto Insane;
    }
    if (hdr->EapVendorId[1] != (0xFF & (simpleconfigHeaderVendorId>>8)))
    {
        goto Insane;
    }
    if (hdr->EapVendorId[2] != (0xFF & (simpleconfigHeaderVendorId)))
    {
        goto Insane;
    }
    if ( hdr->EapVendorTypeHigh != 0 || hdr->EapVendorTypeLow !=
        htons(simpleconfigEapVendorType_SimpleConfig))
    {
        goto Insane;
    }

    /* Assume that we don't have to deal with fragmentation.
     */
    if ( (hdr->SimpleConfigFlags &
            (simpleconfigSimpleConfigFlags_MoreFragments |
             simpleconfigSimpleConfigFlags_LengthField )) != 0 )
    {
        debugf("simpleconfigRecv: received fragmented packet");
        return;
    }

    switch ( hdr->SimpleConfigOpCode )
    {
    case simpleconfigSimpleConfigOpcode_Start: 
        if (simpleconfigS.State != simpleconfigState_eIdentity)
            goto Insane;
        simpleconfigNewState(simpleconfigState_eM1);
        simpleconfigTimerTaskHandler(wsc_drv);
        return;
    case simpleconfigSimpleConfigOpcode_Ack:
        goto BadMsg;
    case simpleconfigSimpleConfigOpcode_Nack: 
        debugf("simpleconfigRecv GOT NACK... Restart! vvvvvvvv");
        simpleconfigDumpNack(Msg, MsgLen);
        debugf("simpleconfigRecv GOT NACK... Restart! ^^^^^^^");
        goto Restart;
    case simpleconfigSimpleConfigOpcode_Msg:
        goto GotMsg;
    case simpleconfigSimpleConfigOpcode_Done: 
        goto BadMsg;
    case simpleconfigSimpleConfigOpcode_FragAck:
        goto BadMsg;
    default:
        break;
    }
    goto Insane;

    GotMsg:;
    switch ( simpleconfigS.State ) {
        case simpleconfigState_eM1:
        {
            int ret;
            simpleconfigS.M2Len = PktLen;
            memset(simpleconfigS.M2, 0, sizeof(simpleconfigS.M2));
            memcpy(simpleconfigS.M2, hdr, PktLen);
            ret =simpleconfigHandleM2(Msg, MsgLen) ;
            if ( ret == 0x99)
            {
                memset( simpleconfigS.ACK, 0, sizeof(simpleconfigS.ACK) );
                simpleconfigS.ACKLen = simpleconfigBuildAck();
                simpleconfigMsgDump( "simpleconfig Send ACK",
                    simpleconfigS.ACK,
                    simpleconfigS.ACKLen
                    );
                simpleconfigPacketSend(wsc_drv, "ACK",
                        simpleconfigS.ACK,
                        simpleconfigS.ACKLen
                        );
                return;
            }
            else if(ret)
                goto BadMsg;
            simpleconfigNewState(simpleconfigState_eM3);
            simpleconfigTimerTaskHandler(wsc_drv);
            return;
        }

        case simpleconfigState_eM3:
            simpleconfigS.M4Len = PktLen;
            memset( simpleconfigS.M4, 0, sizeof(simpleconfigS.M4) );
            memcpy( simpleconfigS.M4, hdr, PktLen );
            if ( simpleconfigHandleM4( Msg, MsgLen ) ) goto BadMsg;
            simpleconfigNewState(simpleconfigState_eM5);
            simpleconfigTimerTaskHandler(wsc_drv);
            return;
        case simpleconfigState_eM5:
            simpleconfigS.M6Len = PktLen;
            memset( simpleconfigS.M6, 0, sizeof(simpleconfigS.M6) );
            memcpy( simpleconfigS.M6, hdr, PktLen );
            if ( simpleconfigHandleM6( Msg, MsgLen ) ) goto BadMsg;
            simpleconfigNewState(simpleconfigState_eM7);
            simpleconfigTimerTaskHandler(wsc_drv);
            return;
        case simpleconfigState_eM7:
            simpleconfigS.M8Len = PktLen;
            memset( simpleconfigS.M8, 0, sizeof(simpleconfigS.M8) );
            memcpy( simpleconfigS.M8, hdr, PktLen );
            if ( simpleconfigHandleM8( Msg, MsgLen ) ) goto BadMsg;
            simpleconfigNewState(simpleconfigState_eDone);
            simpleconfigTimerTaskHandler(wsc_drv);
            printf("We will send WSC_DONE twice\n");
            // add by ygwei
#define SUPP_CONF_FILENAME    "wpa_supplicant.conf"
            FILE *fp = NULL;
            fp = fopen(SUPP_CONF_FILENAME, "w");
            if(fp==NULL)
            {
                printf("Open file failed\n");
            }
            else
            {
                for(i=0;i<simpleconfigS.ResultCnt ;i++)
                {
                    simpleconfigDumpProfile(i);

                    /* Export profile for wpa_supplicant */
                    wsc_write_wpa_supplicant_conf(fp, i==0, 
                            simpleconfigS.Results[i].Ssid,
                            simpleconfigS.Results[i].AuthenticationType,
                            simpleconfigS.Results[i].EncryptionType,
                            simpleconfigS.Results[i].WepTransmitKeyIdx,
                            simpleconfigS.Results[i].NetworkKey,
                            simpleconfigS.Results[i].NetworkKeyLen);
                }
                fclose(fp);
            }

#if 0
            if (wsc_drv->wds_ap_ifname != NULL)
            {
                /* Export profile for WscCmd */
                wsc_write_wsccmd_conf(simpleconfigS.Results.Ssid,
                        simpleconfigS.Results.AuthenticationType,
                        simpleconfigS.Results.EncryptionType,
                        simpleconfigS.Results.WepTransmitKeyIdx,
                        simpleconfigS.Results.NetworkKey,
                        simpleconfigS.Results.NetworkKeyLen);
                
                /* Export profile for Hostapd */
                wsc_write_hostapd_conf(simpleconfigS.Results.Ssid,
                        simpleconfigS.Results.AuthenticationType,
                        simpleconfigS.Results.NetworkKey,
                        simpleconfigS.Results.NetworkKeyLen);
            }
#endif

            simpleconfigS.IsInit = 0;

            wsc_drv->pushButtonPress = 0;

            break; //return; mark by ygwei
        default:
        break;
    }
    goto Insane;

    Insane:
    debugf("simpleconfigRecv: received insane packet\n");

    /* Go into the state machine */
    simpleconfigTimerTaskHandler(wsc_drv);
    return;     // utilize timeout

    BadMsg:
    /* Hopefully we've weeded out any retried messages that should
     * just be ignored... anything else should result in starting over.
     */
    debugf("simpleconfigRecv: received bad msg at state %d\n", 
        simpleconfigS.State);
    #if 0       // should be, but .......
    if ( simpleconfigS.State >= simpleconfigState_eM1 ) {
        goto Restart;
    }
    #endif
    return;     // utilize timeout

    Restart:
//    memset( simpleconfigS.NackPkt, 0, sizeof(simpleconfigS.NackPkt));
//    simpleconfigS.NackPktLen = simpleconfigBuildNack( 
//        simpleconfigS.NackPkt,
//        simpleconfigConfigurationError_NetworkAuthFailure /* ? */
//        );
//    simpleconfigPacketSend(wsc_drv, "NackPkt",
//        simpleconfigS.NackPkt,
//        simpleconfigS.NackPktLen
//        );
//    simpleconfigStartOver();
    return;
}

void InitStateMachine(u8 *own_addr, u8 *ap_addr, u8 method, u8 *pin)
{
    //if (simpleconfigS.IsInit)
    //    return;

    simpleconfigS.IsInit = 1;   /* once only */
    simpleconfigS.LastError = 0;
    simpleconfigS.LastErrMsg[0] = 0;
    simpleconfigS.Failure = 0;
    simpleconfigS.Success = 0;
    simpleconfigS.NStarts = 0;
    simpleconfigS.IsConnected = 0;
    //simpleconfigS.StatusCB = StatusCB;
    simpleconfigS.StatusCB = NULL;
    simpleconfigS.StartSeconds = 0;

    memcpy(simpleconfigS.OurAddr, own_addr, ETH_ALEN);
    memcpy(simpleconfigS.ApAddr, ap_addr, ETH_ALEN);

    if (method == simpleconfigPasswordId_PushButton)
    {
        simpleconfigS.PushButtonMethod = simpleconfigPasswordId_PushButton;
        strcpy(simpleconfigS.Pin, "00000000");
    }
    else
    {
        simpleconfigS.PushButtonMethod = simpleconfigPasswordId_PIN;
        strcpy(simpleconfigS.Pin, pin);
    }

    //simpleconfigS.StartSeconds = mainthreadSecondsSinceBoot();

    /* Because the first three states will be handled by wpa_supplicant,
       we just set the state to Identity state */
    //simpleconfigNewState(simpleconfigState_eStart);
    simpleconfigNewState(simpleconfigState_eIdentity);
}

